#include <SoftwareSerial.h>
#include <Servo.h>
#define ANDROID_CENTER_X 600;
#define ANDROID_CENTER_Y 350;
#define DATA_LENGTH 20;
char serialDate[20];
SoftwareSerial bluetooth(2, 3);

Servo Motor1;
Servo Motor2;

float Motor1_Angle = 90;
float Motor2_Angle = 90;

int Center_x = 0;
int Center_y = 0;

long preTime =0 ;
long predataTime =0 ;

void setup(){
  Serial.begin(57600);
  bluetooth.begin(57600);
  Motor1.attach(9);
  Motor2.attach(10);
  Motor1.write(Motor1_Angle);
  Motor2.write(Motor2_Angle);
  delay(1000);
}
void loop(){
 if(bluetooth.available()){
   bluetooth.readBytesUntil('\n',serialDate,20);
   char *p = serialDate;
   int X = atoi(p);
    p = strchr(p, ',')+1;
   int Y = atoi(p);
   Serial.print(X);
   Serial.print(":");
   Serial.print(Y);
   if(X!=0 || Y!=0){
     Center_x = X - ANDROID_CENTER_X;
     Center_y = Y - ANDROID_CENTER_Y;
   
     Serial.print("  center : ");
     Serial.print(Center_x);
     Serial.print(":");
     Serial.println(Center_y);
     predataTime =millis();
   }
   for(int i=0;i<20;i++){
      serialDate[i]=0;
    }
 }
 long loopTime = millis() -preTime;
 long dataTime = millis() -predataTime;
 if(loopTime >50 && dataTime<500){
    preTime = millis();
    if(Center_x>100)
      Motor1_Angle+=1;
    else if(Center_x<-100)
      Motor1_Angle-=1;
      
    if(Center_y>80)
      Motor2_Angle+=0.8;
    else if(Center_y<-80)
      Motor2_Angle-=0.8;
   Motor1.write((int)Motor1_Angle);
 Motor2.write((int)Motor2_Angle);

 }
}

